#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu(0x68);
double Offset_Setpoint, osp;
signed int speedcount, sc;
unsigned char dl, ds, dt;

double kp, ki, kd;
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, kp, ki, kd, DIRECT);

double sp, si, sd;
double Setpoints, Inputs, Outputs;
PID sPID(&Inputs, &Outputs, &Setpoints, sp, si, sd, DIRECT);

/* 指定 电机1 Arduino 引脚 */
int AN1 = 7;
int AN2 = 8;
int PWMA = 9;

/* 指定 电机2 Arduino 引脚 */
int BN1 = 4;
int BN2 = 5;
int PWMB = 10;

/* TB6612 电机驱动芯片使能引脚 */
int STBY = 6;

/* 第二路编码器引脚, 用于获取电机转动方向 */
int MPB =  8;

/* 记录 MPU6050 DMP 初始化是否成功, 决定 loop 函数是否执行 */
bool dmpReady = false;

/* 记录 MPU6050 DMP 中断状态 */
uint8_t mpuIntStatus;

/* 记录 MPU6050 DMP 初始化是否成功的状态 */
uint8_t devStatus;

/* 用于读取 MPU6050 FIFO 数据 */
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

Quaternion q;
VectorFloat gravity;
float ypr[3];
float angle;                                // 平衡小车倾角
char flag = 0;
char num = 0;
byte inByte;
boolean LED;
float fVol;
String oString = "", nString = "";
String strSS[2];
int iPos;
boolean readCompleted = false;
int iSendType = 0;
